#include "facedir.h"

extern char Model_SMAT[2][100];

CFacedir::CFacedir():reader(30)
{
    image2[0] = 0;
    image2[1] = 0;
    key =0;
    iniR =0;
    iniT =0;
    iniT2 =0;
    printf("---------------------\n\n");
}


void CFacedir::init()
{
    readCalibration();
    InitModelo(&calibration_paramsL, &calibration_paramsR);

    if (iniframe < 0)
    {
	printf("error en la lectura de los videos.\n");
	fflush(stdout);
	exit(-1);
    }
 
    if(save_video)
    {
	std::string buffer = reader.get_pathfile()+format("/video_%d.raw", iniframe);
	videofile = fopen(buffer.c_str(), "w");
	buffer = reader.get_pathfile()+format("/video_%d.txt", iniframe);
	fout = fopen (buffer.c_str(), "w");
    }

    if(save_pose)
    {
	std::string fs_basename = get_basename(reader.get_filename().c_str());

	char file[fs_basename.size()+50];

	sprintf(file, "%s_pose_results.xml", fs_basename.c_str());
	pose_results.reset(cvOpenFileStorage(file, NULL, CV_STORAGE_WRITE), deleteFileStorage);		
    }

    sori = reader.image_size();
    sdest = sori;

    clonekL = cvCreateImage(reader.image_size(), IPL_DEPTH_8U, 3);
    clonekR = cvCreateImage(reader.image_size(), IPL_DEPTH_8U, 3);

    sdest = cvSize(800, 800);
    img_smat[0] = cvCreateImage(sdest, IPL_DEPTH_8U, 1);
    img_smat[1] = cvCreateImage(sdest, IPL_DEPTH_8U, 1);

    roiori = cvRect((sori.width-sdest.width)/2, 0, sdest.width, sori.height);
    roidest = cvRect(0,(sdest.height-sori.height)/2, sdest.width, sori.height);

    vectL = cvCreateMat(3,1, CV_32FC1);
    vectR = cvCreateMat(3,1, CV_32FC1);
    vectRl = cvCreateMat(3,1, CV_32FC1);
    vect_media_L = cvCreateMat(3,1, CV_32FC1);
    vect_media_R = cvCreateMat(3,1, CV_32FC1);
    translation_media_L = cvCreateMat( 3, 1, CV_32FC1 );
    translation_media_R = cvCreateMat( 3, 1, CV_32FC1 );
    translationL = cvCreateMat( 3, 1, CV_32FC1 );
    translationR = cvCreateMat( 3, 1, CV_32FC1 );
    translationRl = cvCreateMat( 3, 1, CV_32FC1 );
    rotation = cvCreateMat( 3, 3, CV_32FC1 );

    printf("Tamaño de la imagen: %d x %d\n",reader.image_size().width,reader.image_size().height);

    lines = cvCreateMat (npoints, ini_opt("lines"), CV_32FC1);
    epilines = cvCreateMat (npoints, 3, CV_32FC1);
    rot = cvCreateMat(3,3, CV_32FC1);
    mask_show = cvCreateMat(npoints, 1, CV_8UC1);


}

void CFacedir::ChangeViewLR()
{
    // pasamos de vectI a vectR, a coordenadas de la cámara derecha
    ::ChangeView(vectL, translationL, &calibration_paramsR, vectR, translationR, rot);
    cvCopy (translationL, translation_media_L);
    cvCopy (vectL, vect_media_L);
}

void CFacedir::init_smat()
{
    //creación del modelo smat
    smatR= load_SMAT_Model(ini_opts("m"), npoints, 19);
    smatL= load_SMAT_Model(ini_opts("m"), npoints, 19);
    print_model_info(smatL);

    // desplazamos los puntos del modelo para que el SMAT use el punto en el centro, no en la esquina.
    desplazar_parche = cvScalar( 15 + roiori.x , 15 - roidest.y, 0, 0);
    cvReshape(points2d[0], &points2d2[0], 2, 0);
    cvReshape(points2d[1], &points2d2[1], 2, 0);
    cvSubS(&points2d2[0], desplazar_parche, &points2d2[0]);
    cvSubS(&points2d2[1], desplazar_parche, &points2d2[1]);

    boost::shared_ptr<CvMat> mp (cvCreateMat(2, points2d[0]->rows, points2d[0]->type), deleteCvMat);
    smat_pts.push_back(mp);
    mp.reset(cvCreateMat(2, points2d[0]->rows, points2d[0]->type), deleteCvMat);
    smat_pts.push_back(mp);
    
    // REdesplazamos los puntos del modelo para que el SMAT use el punto en el centro, no en la esquina.
    cvAddS(&points2d2[0], desplazar_parche, &points2d2[0]);
    cvAddS(&points2d2[1], desplazar_parche, &points2d2[1]);
}

void CFacedir::init2()
{
  
    maskL = cvCreateMat(npoints, 1, CV_8UC1);	    	    
    maskR = cvCreateMat(npoints, 1, CV_8UC1);	    	    
    maskoutL = cvCreateMat(npoints, 1, CV_8UC1);	    	    
    maskoutR = cvCreateMat(npoints, 1, CV_8UC1);
    maskoutL2 = cvCreateMat(npoints, 2, CV_8UC1);
    maskoutR2 = cvCreateMat(npoints, 2, CV_8UC1);

    points2D_reprojectL =  cvCreateMat(npoints, 2, CV_32FC1); 
    points2D_reprojectR =  cvCreateMat(npoints, 2, CV_32FC1); 


    maskoutL = cvCreateMat(npoints, 1, CV_8UC1);	    	    
    maskoutL2 = cvCreateMat(npoints, 2, CV_8UC1);
    masktemp = cvCreateMat(npoints, 1, CV_8UC1);	

    frame = 0;
    quit = false;

    sem_init(&sem1, 0, 0);
    sem_init(&sem2, 0, 0);

    params_left.smat_pts = &smat_pts.at(0);
    params_left.smat = &smatL;
    params_left.src = &srcL;
    params_left.mask_show = maskL;
    params_left.maskout = maskoutL;
    params_left.pointsID = points2d[0];
    params_left.inliers =0;
    params_left.points3D = points3D;
    params_left.cal = &calibration_paramsL;
    params_left.translation = translationL;
    params_left.vect = vectL;
    params_left.vect_media = vect_media_L;
    params_left.quit = 0;
    params_left.desplazar_parche = desplazar_parche;
    params_left.angY = angY;
    params_left.sem1 = &sem1;
    params_left.sem2 = &sem2;
    params_left.n = 0;
    params_left.img_smat = img_smat[0];
    params_left.chesscorners = chesscorners;

    params_right.img_smat = img_smat[1];
    params_right.n = 1;
    params_right.smat_pts = &smat_pts.at(1);
    params_right.smat = &smatR;
    params_right.src = &srcR;
    params_right.mask_show = maskR;
    params_right.maskout = maskoutR;
    params_right.pointsID = points2d[1];
    params_right.inliers =0;
    params_right.points3D = points3D;
    params_right.cal = &calibration_paramsR;
    params_right.translation = translationR;
    params_right.vect = vectR;
    params_right.vect_media = vect_media_R;
    params_right.quit = 0;
    params_right.desplazar_parche = desplazar_parche;
    params_right.angY = angY;
    params_right.sem1 = &sem1;
    params_right.sem2 = &sem2;
    params_right.chesscorners = chesscorners;
}


void CFacedir::init3()
{
    int i;

    //vector inicial
    if (strlen(ini_opts("inivect")))
    {
	float ai, bi, gi;
	sscanf(ini_opts("inivect"), "%f,%f,%f", &ai, &bi, &gi);
    }

    //inicializaci󮠤el buffer de SHOW_IMAGE_BUFFER_SIZE imagenes para poder ir hacia atras
    np_to_show_imgs = 0;
    for (i=0; i<SHOW_IMAGE_BUFFER_SIZE; i++)
    {
	buffer_show_imgsL[i] = cvCreateImage(reader.image_size(), IPL_DEPTH_8U, 3);
	buffer_display_smatL[i] = cvCreateImage(DISPLAY_SIZE, IPL_DEPTH_32F, 3);
	buffer_show_imgsR[i] = cvCreateImage(reader.image_size(), IPL_DEPTH_8U, 3);
	buffer_display_smatR[i] = cvCreateImage(DISPLAY_SIZE, IPL_DEPTH_32F, 3);
    }

    points2d_old[0] = cvCloneMat(points2d[0]);
    points2d_old[1] = cvCloneMat(points2d[1]);

    modificado=0; 
    modificando=0;
    incremento=0.5/180.0*CV_PI;
    incrementopos = 2;
    //float basura;
    vect_2 = cvCloneMat(vectL);

    a = &vect_media_L->data.fl[0];
    b = &vect_media_L->data.fl[1];
    c = &vect_media_L->data.fl[2];

    x = &translation_media_L->data.fl[0];
    y = &translation_media_L->data.fl[1];
    z = &translation_media_L->data.fl[2];

    a2 = &vect_2->data.fl[0];
    b2 = &vect_2->data.fl[1];
    c2 = &vect_2->data.fl[2];

    oldtrans = cvCloneMat(translation_media_L);
    oldvect = cvCloneMat(vect_media_L);
}

void CFacedir::readCalibration()
{
    //búsqueda del fichero de calibración y 
    //  leer los parámetros de calibración
    std::string buffer = reader.get_pathbasename()+".cal";
    printf("fichero de\ncalibración: %s\n", buffer.c_str());

    CvFileStorage* fs = cvOpenFileStorage( buffer.c_str(), 0, CV_STORAGE_READ );
    calibration_paramsL.Fundamental = (CvMat *) cvReadByName(fs, NULL, "Fundamental");
    calibration_paramsL.R = (CvMat *) cvReadByName(fs, NULL, "R");
    calibration_paramsL.T = (CvMat *) cvReadByName(fs, NULL, "T");
    calibration_paramsL.KK = (CvMat *) cvReadByName(fs, NULL, "Fl");
    calibration_paramsR.KK = (CvMat *) cvReadByName(fs, NULL, "Fr");
    
    cp.block_size = cvReadInt(cvGetFileNodeByName( fs, NULL, "block_size"), 19) ;
    cp.k_harris = cvReadReal(cvGetFileNodeByName( fs, NULL, "k_harris"), 0.2375);
    cp.min_distance = cvReadInt(cvGetFileNodeByName( fs, NULL, "min_distance"), 30);
    cp.quality_level_max=cvReadReal(cvGetFileNodeByName(fs, NULL, "quality_level_max"),0.006);
    cp.quality_level_min=cvReadReal(cvGetFileNodeByName(fs, NULL, "quality_level_min"),0.0000400);
    cvReleaseFileStorage( &fs );
}


void CFacedir::draw_points()
{
    int i;
    if (!image2[0])
	image2[0] = cvCloneImage(reader.img(0));
    else
	cvCopy(reader.img(0), image2[0]);
    if (!image2[1])
	image2[1] = cvCloneImage(reader.img(1));
    else
	cvCopy(reader.img(1), image2[1]);

    for (i=0; i<npoints; i++)
    {
	cvCircle(image2[0],cvPoint((int)cvmGet(points2d[0],i,0), (int)cvmGet(points2d[0],i,1))
		 , 7,  CV_RGB(0,0,0), 1, CV_AA, 0);
	cvCircle(image2[1],cvPoint((int)cvmGet(points2d[1],i,0),(int)cvmGet(points2d[1],i,1))
		 , 7,  CV_RGB(0,0,0), 1, CV_AA, 0);
    }
}


void CFacedir::ResizeImages()
{
    for (int i=0; i<2; i++)
    {
	cvZero(img_smat[i]);
	cvSetImageROI(reader.img(i), roiori);
	cvSetImageROI(img_smat[i], roidest);
    
	cvCopy(reader.img(i), img_smat[i]);// = imgs->left;
    
	cvResetImageROI(reader.img(i));
	cvResetImageROI(img_smat[i]);
    }
}


void CFacedir::copy_backup()
{
    cvCopy (points2d[0], points2d_old[0]);
    cvCopy (points2d[1], points2d_old[1]);
    cvCopy(translation_media_L, oldtrans);
    cvCopy(vect_media_L, oldvect);
    np_to_show_imgs++;
    np_to_show_imgs %= SHOW_IMAGE_BUFFER_SIZE;
    clonekL = buffer_show_imgsL[np_to_show_imgs];
    clonekR = buffer_show_imgsR[np_to_show_imgs];
    displayL = buffer_display_smatL[np_to_show_imgs];
    displayR = buffer_display_smatR[np_to_show_imgs];
}

void CFacedir::MostrarModelo()
{
    int i;
    int index_punto =0;
    struct mousecalldata_t paramsMouseCall[2];
    IplImage *image3[2];

    cvSet(mask_show, cvScalar(1));

    //GetStereoPoint(points2d[0], points2d[1], &calibration_paramsL, &calibration_paramsR, points3D);
    cvComputeCorrespondEpilines(points2d[0], 1, calibration_paramsR.Fundamental, epilines);
    LineasModelo(points3D, lines);
    CenterModel(points3D);    
    
    Modelo(points3D, points2d[0], mask_show, calibration_paramsL.KK, rotation, translationL, vectL);
    ChangeViewLR();
    offsetmodel = cvCloneMat(translationL);

    for (i=0; i<2; i++)
    {
	image2[i] = cvCreateImage(reader.image_size(), IPL_DEPTH_8U, 3);
	cvConvertImage(reader.img(i), image2[i]);
	image3[i] = cvCloneImage(image2[i]);

	paramsMouseCall[i].points = points2d[i];
	paramsMouseCall[i].pointsI = points2d[0];
	paramsMouseCall[i].pointsD = points2d[1];
	paramsMouseCall[i].points3D = points3D;
	paramsMouseCall[i].epilines = epilines;
	paramsMouseCall[i].vectL = vectL;
	paramsMouseCall[i].translationL = translationL;
	paramsMouseCall[i].lines = lines;
	paramsMouseCall[i].calL = &calibration_paramsL;
	paramsMouseCall[i].calR = &calibration_paramsR;
	paramsMouseCall[i].image = image3;
	paramsMouseCall[i].image2 = image2;
	paramsMouseCall[i].ID = i;
	paramsMouseCall[i].index = &index_punto;
	paramsMouseCall[i].angY = angY;
	paramsMouseCall[i].mask_show = mask_show;
	paramsMouseCall[i].offsetmodel = offsetmodel;
	paramsMouseCall[i].chesscorners = chesscorners;
	paramsMouseCall[i].pface = this;
	//cvResizeWindow(window[i], 600,600);

	cvSetMouseCallback(window[i], on_mouse, (void*)&paramsMouseCall[i]);    
    }
    
    DibujarStereo();

    for (i=0; i<2; i++)
    {
	cvShowImage(window[i], image2[i]);
    }
    
    cvSet(mask_show, cvScalar(1));
    ::MostrarModelo(paramsMouseCall);

    npoints = points3D->rows;

    //GetStereoqqPoint(points2d[0], points2d[1], &calibration_paramsL, &calibration_paramsR, points3D);
    LineasModelo(points3D, lines);

    //centra el modelo sobre el punto 0 para un correcto uso de cvPOSIT / cvProjectPoints2
    CenterModel(points3D);    
    
    cvReleaseImage(&image3[0]);
    cvReleaseImage(&image3[1]);
    cvSetMouseCallback(window[0], NULL, NULL);
    cvSetMouseCallback(window[1], NULL, NULL);
    
}


void CFacedir::DibujarStereo(int index)
{
    ::DibujarModelo3D(image2[0], points3D, &calibration_paramsL, translationL, vectL, lines, mask_show );
    ::DibujarModelo3D(image2[1], points3D, &calibration_paramsR, translationR, vectR, lines, mask_show);
    //::DibujarStereo(image2, 2, points2d[0], points2d[1], chesscorners, NULL, index);
}

void CFacedir::DemostrarModelo()
{
    cvSet(mask_show, cvScalar(1));
    Modelo(points3D, points2d[0], mask_show, calibration_paramsL.KK, rotation, translationL, vectL);

    offsettrans = cvCloneMat(translationL);
    offsettrans2 = cvCloneMat(translationL);
    x2 = &offsettrans->data.fl[0];
    y2 = &offsettrans->data.fl[1];
    z2 = &offsettrans->data.fl[2];
    printf ("offsettrans: frame %d -> (%f %f %f)\n", lastframe, *x2, *y2, *z2);

    cvNamedWindow("D Modelo3D Der", 0);
    cvResizeWindow("D Modelo3D Der", 600,600);
    cvNamedWindow("D Modelo3D", 0);
    cvResizeWindow("D Modelo3D", 600,600);

    cvSet(mask_show, cvScalar(1));

    cvConvertImage(reader.img(0), clonekL);
    ::DibujarModelo3D(clonekL, points3D,  &calibration_paramsL, translationL, vectL,  lines, mask_show);
    cvShowImage("D Modelo3D", clonekL);
    // obtenemos vectR y translationR en coordenadas de la cámara derecha
    ChangeView(vectL, translationL, &calibration_paramsR, vectR, translationR, rot);
    //Modelo(points3D, points2d[1], NULL, calibration_paramsR.KK, rotation, translationR, vectR);
    cvConvertImage(reader.img(1), clonekR);
    ::DibujarModelo3D(clonekR, points3D,  &calibration_paramsR, translationR, vectR,  lines, mask_show);
    cvShowImage("D Modelo3D Der", clonekR);

    // obtenemos vectR y translationR en coordenadas de la cámara derecha
    // 	ChangeView(vectR, translationR, &calibration_paramsL, vectL, translationL, rot);
    //::DibujarModelo3D(clonekL, points3D,  &calibration_paramsL, translationL, vectL,  lines, mask_show);
    // 	cvShowImage("D Modelo3D", clonekL);

    cvConvertImage(reader.img(1), clonekL);
    DibujarModelo(clonekL, points2d[0], &calibration_paramsL, translationL, vectL,  lines, mask_show);
    cvShowImage(Model_SMAT[0], clonekL);
    cvWaitKey(0);

    cvDestroyWindow("D Modelo3D");
    cvDestroyWindow("D Modelo3D Der");

    cvReleaseImage(&clonekL);
    cvReleaseImage(&clonekR);
}

void CFacedir::facedir()
{
    //se pasa el vector de rotación de coordenadas de una cámara a la otra
    ChangeView(vectR, translationR, &calibration_paramsL, vectRl, translationRl, rot);

    //media de los dos vectores
    //media de los dos vectores
    cvAddWeighted(vectL, (float)inliersL/(float)(inliersL+inliersR), 
		  vectRl, (float)inliersR/(float)(inliersR+inliersL), 0, 
		  vect_media_L);
    //cvAddWeighted(vectL, errorL/(errorL+errorR), vect, errorR/(errorL+errorR), 0, vectL);
    cvAddWeighted(translationL, (float)inliersL/(float)(inliersL+inliersR), 
		  translationRl, (float)inliersR/(float)(inliersR+inliersL), 0, 
		  translation_media_L);
    if(frame>=0)
    {
	if (inliersL < MIN_INLIERS &&   inliersR > MIN_INLIERS)
	{
	    //usa el valor de la otra cámara, de la derecha
	    ChangeView(vectR, translationR, &calibration_paramsL, vectL, translationL, rot);
	    cvProjectPoints2(points3D, vectL, translationL, calibration_paramsL.KK, NULL, points2D_reprojectL);
	    cvCopy(vectL, vect_media_L);
	    cvCopy(translationL, translation_media_L);
	    cvCopy(vectL, vectRl);
	    cvCopy(translationL, translationRl);

	    cvSet(maskoutL, cvScalar(255), maskL);
	    smatL->remove_last_added(maskoutL);
		
	    //update the value of the outlier points with the reprojected value
	    cvCopy(points2D_reprojectL, points2d[0], 0);
	}
	else
	{
	    if(inliersL > MIN_INLIERS2)
	    {
		if (frame == 128)
		    key=0;
		cvProjectPoints2(points3D, vectL, translationL, calibration_paramsL.KK, NULL, points2D_reprojectL);
		    
		cvCmpS(maskoutL, 0, maskoutL, CV_CMP_EQ);//invert the mask
		cvAnd(maskoutL, maskL, maskoutL);
		smatL->remove_last_added(maskoutL);
		
		//update the value of the outlier points with the reprojected value
		cvRepeat(maskoutL, maskoutL2);
		cvCopy(points2D_reprojectL, points2d[0], maskoutL2);
	    }
	    else
	    {
		cvSet(maskoutL, cvScalar(255), maskL);
		smatL->remove_last_added(maskoutL);
	    }

	}
    }


    if(frame>=0 && inliersR)
    {
	if (inliersR < MIN_INLIERS &&   inliersL > MIN_INLIERS)
	{
	    //usa el valor de la otra cámara, de la izquierda
	    ChangeView(vectL, translationL, &calibration_paramsR, vectR, translationR, rot);
	    cvProjectPoints2(points3D, vectR, translationR, calibration_paramsR.KK, NULL, points2D_reprojectR);
	    cvCopy(vectL, vect_media_L);
	    cvCopy(translationL, translation_media_L);
	    cvCopy(vectL, vectRl);
	    cvCopy(translationL, translationRl);
		
	    cvSet(maskoutR, cvScalar(255), maskR);
	    smatR->remove_last_added(maskoutR);
		
	    //update the value of the outlier points with the reprojected value
	    cvCopy(points2D_reprojectR, points2d[1], 0);
	}
	else
	{
	    if(inliersR > MIN_INLIERS2)
	    {
		cvProjectPoints2(points3D, vectR, translationR, calibration_paramsR.KK, NULL, points2D_reprojectR);
		
		cvCmpS(maskoutR, 0, maskoutR, CV_CMP_EQ);//invert the mask
		cvAnd(maskoutR, maskR, maskoutR);
		smatR->remove_last_added(maskoutR);
		
		//update the value of the outlier points with the reprojected value
		cvRepeat(maskoutR, maskoutR2);
		cvCopy(points2D_reprojectR, points2d[1], maskoutR2);
	    }
	    else
	    {
		cvSet(maskoutR, cvScalar(255), maskR);
		smatR->remove_last_added(maskoutR);
	    }
	}
    }

    ChangeView(vect_media_L, translation_media_L, &calibration_paramsR, 
	       vect_media_R, translation_media_R, rot);
    //ChangeView(vectR, translationR, &calibration_paramsL, vectRl, translationRl, rot);

}


void CFacedir::facedir_control()
{
    if (frame2 == endframe)
    {
	printf("\n------------------------------\nEnd frame %d reached\n------------------\n", frame2);
	cvWaitKey(0);
	printf("\n------------------------------\nEnd frame %d reached\n------------------\n", frame2);
	cvWaitKey(0);
	wait = 0;
    }

    do
    {
	printf(CRED "inliersL : %d, error: %f" CNORMAL "\n", inliersL, params_left.error);
	CvMat *vect2 = cvCreateMat(3, 1, CV_32FC1);
	cvScale(vectL, vect2, 180.0/CV_PI);
	printf("vectL: ");
	print_arr(vect2, true, "%1.2f,");
	printf("translationL: ");
	print_arr(translationL, true, "%3f, ");
	print_arr(params_left.maskout, 1, "%3.0f, ");
	print_arr(masktemp, 1, "%3.0f, ");
	cvReleaseMat(&vect2);

	cvConvertImage(reader.img(0), clonekL);
	::DibujarModelo(clonekL, points2d[0], &calibration_paramsL, translation_media_L, 
			vect_media_L,lines, params_left.mask_show, params_left.maskout, CV_RGB(00,60,200), -1);
	::DibujarModelo3D(clonekL, params_left.points3D,  &calibration_paramsL, translation_media_L, 
			  vect_media_L,
			  lines, params_left.mask_show,  params_left.maskout, CV_RGB(255,0,0), -1, false);
	cvShowImage(window[0], clonekL);
	cvShowImage(Model_SMAT[0], displayL);

#ifdef SHOW_RIGHT
	cvConvertImage(reader.img(1), clonekR);
	::DibujarModelo(clonekR, points2d[1], &calibration_paramsR, translation_media_R, 
			vect_media_R,lines, params_right.mask_show, params_right.maskout, CV_RGB(00,60,200), -1);
	::DibujarModelo3D(clonekR, params_right.points3D,  &calibration_paramsR, translation_media_R, 
			  vect_media_R,
			  lines, params_right.mask_show,  params_right.maskout, CV_RGB(255,0,0), -1, false);
	cvShowImage(window[1], clonekR);
	cvShowImage(Model_SMAT[1], displayR);
#endif
	

	np_to_show_imgs2 = np_to_show_imgs;

	modificando = 0;
	while ((key = bucle_cvWaitKey(&wait)) > 0)
	{
	    if (!(key & 0xFF0000))
	    {
		switch (key)
		{
		case 'w':
		case 'W':
		    key = -2;
		    break;
		case 'I':
		case 'i':
		    print_model_state(smatR);	    
		    break;
		case  0xFF51: //BACK ARROW
		    //ir hacia atras en las imagenes a mostrar
		    np_to_show_imgs2 = (np_to_show_imgs2-1+SHOW_IMAGE_BUFFER_SIZE) 
			% SHOW_IMAGE_BUFFER_SIZE;
		    printf("image frame diff: %d / %d\n", reader.head(0)->nframe +(
										   (np_to_show_imgs2 - np_to_show_imgs - SHOW_IMAGE_BUFFER_SIZE )
										   % SHOW_IMAGE_BUFFER_SIZE), reader.head(0)->nframe);
		    cvShowImage(window[0], buffer_show_imgsL[np_to_show_imgs2]);
		    cvShowImage(Model_SMAT[0], buffer_display_smatL[np_to_show_imgs2]);
		    cvShowImage(window[1], buffer_show_imgsR[np_to_show_imgs2]);
		    cvShowImage(Model_SMAT[1], buffer_display_smatR[np_to_show_imgs2]);
		    break;
		case  0xFF53: //NEXT ARROW
		    //ir hacia atras en las imagenes a mostrar
		    np_to_show_imgs2 = (np_to_show_imgs2+1) % SHOW_IMAGE_BUFFER_SIZE;
		    printf("image frame diff: %d / %d\n", reader.head(0)->nframe +(
										   (np_to_show_imgs2 - np_to_show_imgs - SHOW_IMAGE_BUFFER_SIZE )
										   % SHOW_IMAGE_BUFFER_SIZE), reader.head(0)->nframe);
		    cvShowImage(window[0], buffer_show_imgsL[np_to_show_imgs2]);
		    cvShowImage(Model_SMAT[0], buffer_display_smatL[np_to_show_imgs2]);
		    cvShowImage(window[1], buffer_show_imgsR[np_to_show_imgs2]);
		    cvShowImage(Model_SMAT[1], buffer_display_smatR[np_to_show_imgs2]);
		    break;

		case 0xFF52: //UP
		    break;
		case 0xFF54: //DOWN
		    break;
		case 's':
		case 'S':
		    SaveModel();
		    break;
		default:
		    printf("Undefined key %d - (%X)\n", key, key);
		}
	    }
	    else
	    {
		switch(key)
		{
		    //SHIFT + ...
		case  0x1FF51: //left
		    *c += -incremento;
		    break;
		case  0x1FF53: //right
		    *c += incremento;
		    break;

		    //SHIFT
		case 0x1FF52: //UP
		    *z += -incrementopos;
		    break;
		case 0x1FF54: //DOWN
		    *z += incrementopos;
		    break;

		    //ctrl + ...
		case  0x4FF51: //left
		    *b += incremento;
		    break;
		case  0x4FF53: //right
		    *b += -incremento;
		    break;
		case 0x4FF52: //UP
		    *a += -incremento;
		    break;
		case 0x4FF54: //DOWN
		    *a += incremento;
		    break;

		    //ALT + ...
		case  0x8FF51: //left
		    *x += -incrementopos;
		    break;
		case  0x8FF53: //right
		    *x += incrementopos;
		    break;
		case 0x8FF52: //UP
		    *y += -incrementopos;
		    break;
		case 0x8FF54: //DOWN
		    *y += incrementopos;
		    break;
		case 0x40000 | 'p':
		case 0x40000 | 'P':
		    printf("copy last\n");
		cvCopy(oldtrans, translation_media_L);
		cvCopy(oldvect, vect_media_L);
		break;
		default:
		    printf("Undefined key %d(%X)\n", key, key);
		}
		modificado = modificando = 1;
		key = 1;
		wait=0;
		break;
		if (key<=0)
		    break;
	    } //if (key & 0xFF0000)
	}  //while ((key = bucle_cvWaitKey(&wait)) > 0)
	if(key < 0)
	{
	    quit = true;
	    params_left.quit = true;
	    params_right.quit = true;
	    
	}
	if (modificando)
	    ChangeView(vect_media_L, translation_media_L, &calibration_paramsR, 
		       vect_media_R, translation_media_R, rot);

    } while (modificando && key > 0);

    if (modificado)
    {
	ChangeView(vect_media_L, translation_media_L, &calibration_paramsR, 
		   vect_media_R, translation_media_R, rot);

	cvCopy(vect_media_L, vectL);
	cvCopy(translation_media_L, translationL);
	cvCopy(vect_media_R, vectR);
	cvCopy(translation_media_R, translationR);

	modificado =0;
	printf("-- modificado --\n");
	fflush(stdout);

 	cvProjectPoints2(params_left.points3D, vect_media_L, translation_media_L, 
			 calibration_paramsL.KK, NULL, points2D_reprojectL);
	//update the value of the outlier points with the reprojected value
	cvCopy(points2D_reprojectL, points2d[0]);

 	cvProjectPoints2(params_right.points3D, vect_media_R, translation_media_R, 
			 calibration_paramsR.KK, NULL, points2D_reprojectR);
	//update the value of the outlier points with the reprojected value
	cvCopy(points2D_reprojectR, points2d[1]);
    }

    if (frame > 0)
    {	
	if (save_video && key!=-1)
	{
	    //write results
	    if (0)
	    {
		IplImage *final2 = cvCreateImage(cvGetSize(displayL), IPL_DEPTH_8U, 3);
		cvConvertImage(displayL, final2);

		int w = 500;
		int h = (int)((float)final2->height / (float) final2->width * (float)w); 
		cvSetImageROI(clonekL, cvRect(0, cvGetSize(clonekL).height-h, w, h));
		//cvResize(final2, clonekL, CV_INTER_CUBIC);
		cvConvertImage(displayL, final2);
		cvSetImageROI(clonekL, cvRect(0, 800-h, w, h));
		cvResize(final2, clonekL, CV_INTER_CUBIC);
		cvResetImageROI(clonekL); 

		cvReleaseImage(&final2);
	    }
	    //cvWriteFrame(video_results_FaceDir.get(), clonekL);
	    if (videofile)
		fwrite(clonekL->imageData, clonekL->imageSize, 1, videofile);


	    fprintf(fout, "%d %f %f %f %f %f %f\n",
		    frame2,
		    cvmGet(vect_media_L,0,0), cvmGet(vect_media_L,1,0),cvmGet(vect_media_L,2,0),
		    cvmGet(translation_media_L,0,0), cvmGet(translation_media_L,1,0),
		    cvmGet(translation_media_L,2,0));
	    fflush(fout);

	    printf("---------------- saving frame %d --------------\n", frame);
	}
    }
}
 


bool CFacedir::SaveModel()
{
    CvMat *angY2;
    CvFileStorage* fs;
    
    cvProjectPoints2(points3D, vectL, translationL, calibration_paramsL.KK, NULL, points2d[0]);
    cvProjectPoints2(points3D, vectR, translationR, calibration_paramsR.KK, NULL, points2d[1]);

    std::string buffer = reader.get_pathbasename()+ format(".dat.%d", frame2);
    fs = cvOpenFileStorage( buffer.c_str(), 0, CV_STORAGE_WRITE );
    cvWrite(fs, "pointsL", points2d[0]);
    cvWrite(fs, "pointsR", points2d[1]);
    cvWrite(fs, "points3D", points3D);
    angY2 = cvCloneMat(angY);
    cvScale(angY2,angY2, 180.0F/CV_PI);
    cvWrite(fs, "angY", angY2);
    cvWrite(fs, "chesscorners", chesscorners);
    cvReleaseFileStorage(&fs);
    cvReleaseMat(&angY2);

    printf("Model saved to %s\n", buffer.c_str());
    return true;
}
